/**
 * @file kalman.h
 * @author LinYun (1828951982@qq.com)
 * @brief kalman filter header file
 * @version 0.1
 * @date 2024-08-01
 *
 * @copyright Copyright (c) 2024
 *
 */
#pragma once

#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

namespace kf
{
    class KalmanFilter
    {
    private:
        /* data */
        int _stateNum = 0;
        int _measureNum = 0;
        int _controlNum = 0;
        float _T = 0.0;

        MatrixXd _transitionMat;
        MatrixXd _measureMat;
        MatrixXd _controlMat;
        MatrixXd _postCovMat;
        MatrixXd _proNoiseCovMat;
        MatrixXd _meaNoiseCovMat;

        VectorXd _stateVec;
        VectorXd _controlVec;
        VectorXd _processNoise;
        VectorXd _measureNoise;

    public:
        KalmanFilter(int stateNum = 0, int measurementNum = 0, int controlNum = 0, float T = 1.0);
        ~KalmanFilter();
        MatrixXd predict();
        MatrixXd correct(const VectorXd& measureVec);
        bool setTansMat(const MatrixXd& transitionMat);
        bool setMeasMat(const MatrixXd& measurementnMat);
        bool setQ(const MatrixXd& proNoiseCovMat);
        bool setR(const MatrixXd& meaNoiseCovMat);
        void info();
    };
}
